#include "lidar_fusion_core.h"

LidarFusion::LidarFusion():nh_(""), private_nh_("~"), listener_(buffer_){
    private_nh_.param<std::string>("input_front_lidar_topic_", input_front_lidar_topic_, "");
    private_nh_.param<std::string>("input_front_lidar_topic_frame_id", input_front_lidar_topic_frame_id, "");
    private_nh_.param<std::string>("input_back_lidar_topic_", input_back_lidar_topic_, "");
    private_nh_.param<std::string>("input_back_lidar_topic_frame_id", input_back_lidar_topic_frame_id, "");
    private_nh_.param<std::string>("output_confusion_lidar_topic_", output_confusion_lidar_topic_, "");
    private_nh_.param<std::string>("output_confusion_lidar_frame_id", output_confusion_lidar_frame_id, "");
}

LidarFusion::~LidarFusion(){}

void LidarFusion::run(){
    front_lidar_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_front_lidar_topic_, 1, &LidarFusion::front_lidar_call_back, this);
    back_lidar_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_back_lidar_topic_, 1, &LidarFusion::back_lidar_call_back, this);
    confusion_lidar_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_confusion_lidar_topic_, 1);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &LidarFusion::process, this, false, true);
    ros::spin();
}


void LidarFusion::front_lidar_call_back(const sensor_msgs::PointCloud2::ConstPtr& msg){
    try{
        geometry_msgs::TransformStamped transform_stamed = buffer_.lookupTransform(output_confusion_lidar_frame_id, input_front_lidar_topic_frame_id, ros::Time(0));
        geometry_msgs::Transform transform;
        transform.translation = transform_stamed.transform.translation;
        transform.rotation = transform_stamed.transform.rotation;

        Eigen::Matrix4f eigen_transform;
        pcl_ros::transformAsMatrix(transform, eigen_transform);
        pcl_ros::transformPointCloud(eigen_transform, *msg, front_lidar_pc_trans);
        front_lidar_pc_trans.header.frame_id = output_confusion_lidar_frame_id;
        // std::cout<<"sub front lidar"<<std::endl;
    }catch (tf2::TransformException &ex)
    {
        ROS_WARN("[%s] front lidar lookupTransform error, %s", _APP_NAME_, ex.what());
    }
}


void LidarFusion::back_lidar_call_back(const sensor_msgs::PointCloud2::ConstPtr& msg){
    try{
        geometry_msgs::TransformStamped transform_stamed = buffer_.lookupTransform(output_confusion_lidar_frame_id, input_back_lidar_topic_frame_id, ros::Time(0));
        geometry_msgs::Transform transform;
        transform.translation = transform_stamed.transform.translation;
        transform.rotation = transform_stamed.transform.rotation;

        Eigen::Matrix4f eigen_transform;
        pcl_ros::transformAsMatrix(transform, eigen_transform);
        pcl_ros::transformPointCloud(eigen_transform, *msg, back_lidar_pc_trans);
        back_lidar_pc_trans.header.frame_id = output_confusion_lidar_frame_id;
        // std::cout<<"sub back lidar"<<std::endl;
    }catch (tf2::TransformException &ex)
    {
        ROS_WARN("[%s] back lidar lookupTransform error, %s", _APP_NAME_, ex.what());
    }
}

void LidarFusion::process(const ros::TimerEvent& e){
    // std::cout<<"lidar fusioning "<<std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_confusion_pc(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_back_lidar_pc(new pcl::PointCloud<pcl::PointXYZ>());
    //时间同步
    // if(fabs(ros::Time().now().toSec()-front_lidar_pc_trans.header.stamp.toSec())<0.5){
    //     pcl::PointCloud<pcl::PointXYZ>::Ptr lidar_pc2(new pcl::PointCloud<pcl::PointXYZ>());
    //     pcl::fromROSMsg(front_lidar_pc_trans, *lidar_pc2);
    //     *pcl_confusion_pc = *pcl_confusion_pc + *lidar_pc2;
    // }else{
    //     ROS_WARN("[%s] front lidar timeout", _APP_NAME_);
    // }


    // if(fabs(ros::Time().now().toSec()-back_lidar_pc_trans.header.stamp.toSec())<0.5){
    //     pcl::fromROSMsg(back_lidar_pc_trans, *pcl_back_lidar_pc);
    // }else{
    //     ROS_WARN("[%s] back lidar timeout", _APP_NAME_);
    // }
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr lidar_pc2(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(front_lidar_pc_trans, *lidar_pc2);
    *pcl_confusion_pc = *pcl_confusion_pc + *lidar_pc2;



    pcl::fromROSMsg(back_lidar_pc_trans, *pcl_back_lidar_pc);
   

    // 叠加点云
    *pcl_confusion_pc = *pcl_confusion_pc + *pcl_back_lidar_pc;

    // 发布
    sensor_msgs::PointCloud2 ros_pc2_out;
    pcl::toROSMsg(*pcl_confusion_pc, ros_pc2_out);
    ros_pc2_out.header.stamp = ros::Time::now();
    ros_pc2_out.header.frame_id = output_confusion_lidar_frame_id;
    confusion_lidar_pub_.publish(ros_pc2_out);
}
